#include "tools.h"
#include <iostream>
#include <itpp/config.h>
#include <itpp/itbase.h>
#include <itpp/base/algebra/schur.h>

/* calculate (\mat{I_n}-\mat{M})^{-1} 
   by series expansion, when |M| is small,
   i.e, (\mat{I_n}-\mat{M})^{-1} = sum_{k=0}^{n} \mat{M}^k */
mat inv_expand(mat M, int order)
{
	mat tmp = mat_pow(M,0);
	if (0 == order)
	{
		return tmp;
	}
	else
	{
		for (int i = 1; i <= order; ++i)
		{
			tmp = tmp + mat_pow(M,i);
		}
	}
	return tmp;
}

/* Calculate \mat{T}
   \mat{T} =  (\mat{I}+\frac{\tau}{2}\mat{M})^{-1}*(\mat{I}-\frac{\tau}{2}\mat{M}) 
           =  \mat{I} + \mat{T_a}
   Method:
   \tau = \Delta t /2^N \quad (N=20)
   \mat{T}_a =-\tau \mat{M}
              +\frac{1}{2}\tau^2\mat{M}^2
              -\frac{1}{4}\tau^3\mat{M}^3 
   for {iter = 0;iter< N;iter++}
     \mat{T}_a = 2\mat{T}_a + \mat{T}_a \times \mat{T}_a ;
   end
   \mat{T} = \mat{I} + \mat{T}_a;*/
mat spim(const mat& M, const double& t, const int& N)
{
	// set the bumber of the bins for the time interval
	int m = 1 << N;
	double tau = -t/m;

	// compute the matrix Ta = T - I
	size_t dim = M.n_rows;
	mat Ta = -tau*M + (0.5*tau*M) * (0.5*tau*M) * (2*eye(dim, dim) - tau*M);

	for (int i = 0; i < N; ++i)
	{
		Ta = 2*Ta + Ta * Ta;
	}

	return eye(dim, dim) + Ta;
}

mat mat_pow(mat M, int order)
{
	int n_size = M.n_rows;
	mat tmp = eye(n_size, n_size);
	if (0 == order)
	{
		return tmp;
	}
	else
	{
		for (int i = 0; i < order; ++i)
		{
			tmp *= M;
		}
	}
	return tmp;
}

void get_time(char* currtime)
{
	time_t rawtime;
	struct tm * timeinfo;
	time ( &rawtime );
	timeinfo = localtime ( &rawtime ); 
	int year,month,day,hour,minute,second;
	year = 1900+timeinfo->tm_year;
	month = 1+timeinfo->tm_mon;
	day = timeinfo->tm_mday;
	hour = timeinfo->tm_hour;
	minute = timeinfo->tm_min;
	second = timeinfo->tm_sec;
	sprintf(currtime,"%02d-%02d-%02d %02d:%02d:%02d",year,month,day,hour,minute,second);
	return;
}

int findCeilMax(std::vector<mat> vMat)
{
	double m_max = 0;
	for (int i = 0; i < vMat.size(); ++i)
	{
		if( vMat[i].max() > m_max)
			m_max = vMat[i].max();
	}

	return ceil(m_max);
}

int findFloorMin(std::vector<mat> vMat) 
{
	double m_min = 0;
	for (int i = 0; i < vMat.size(); ++i)
	{
		if( vMat[i].min() < m_min)
			m_min = vMat[i].min();
	}

	return floor(m_min);
} 

double Lnorm(mat M)
{
	double lnorm=0;
	for (int i = 1; i < M.n_rows; ++i)
	{
		for (int j = 0; j < i; ++j)
		{
			if (fabs(M(i,j)) > lnorm)
				lnorm = fabs(M(i,j));
		}
	}
	return lnorm;
}

itpp::mat matConveter(arma::mat M)
{
	itpp::mat _M(M.n_rows, M.n_cols);
	for (int i = 0; i < M.n_rows; ++i)
	{
		for (int j = 0; j < M.n_cols; ++j)
		{
			_M(i,j) = M(i,j);
		}
	}

	return _M;
}

arma::mat matConveter(itpp::mat M)
{
	arma::mat _M(M.rows(), M.cols());
	for (int i = 0; i < M.rows(); ++i)
	{
		for (int j = 0; j < M.cols(); ++j)
		{
			_M(i,j) = M(i,j);
		}
	}

	return _M;
}

void Schur(mat& U, mat X)
{
	itpp::mat _X;
	_X = matConveter(X);
	itpp::mat _U, _T;
	itpp::schur(_X,_U,_T);
	U = matConveter(_U);
}

